from abc import ABC,abstractmethod
from px4_msgs.msg import VehicleStatus
import enum
from geopy.distance import geodesic
import math
import numpy as np
from std_msgs.msg import Int16
from sls_controller.sls_pxOffboard import PxOffboard

from sls_controller.sls_publicResource import PublicResource

class MissionsState(enum.Enum):
    WAITING = 0
    RUNNING = 1
    DONE = 2
    ABORT = 3

class Missions(ABC):
    def __init__(self,publicResource:PublicResource,datasets:dict):
        self.datasets=datasets
        self.state = MissionsState.WAITING
        self.foreCheckTimes = -1
        self.positiveAbortFlag = False
        self.waitoldtim=0
        self.waitnewtime=0
        self.publicResource= publicResource
        self.abort_callback = None
        self.finish_callback = None

    #给调度器调用的方法
    @abstractmethod
    def fore_check(self) -> bool:
        pass

    @abstractmethod
    def done_check(self) -> bool:
        pass

    @abstractmethod
    def abort_check(self):
        pass

    @abstractmethod
    def mission_enter(self):
        pass
    @abstractmethod
    def mission_action(self):
        pass
    @abstractmethod
    def mission_exit(self):
        pass

    @abstractmethod
    def mission_abort(self):
        pass

    #自我调用的方法
    def mission_blockWait(self, ms:int) -> bool:
        if self.waitoldtime == 0:
            self.waitoldtime = self.publicResource.get_clock().now().nanoseconds / 1000000
        else:
            self.waitnewtime = self.publicResource.get_clock().now().nanoseconds / 1000000    
            if self.waitnewtime - self.waitoldtime >= ms:
                self.waitoldtime = 0
                return True
            else:
                return False
            
    def positive_abort(self):
        self.positiveAbortFlag = True
    
    def positive_abort_check(self):
        if self.positiveAbortFlag:
            self.positiveAbortFlag = False
            return True
        else:
            return False
        



# class ServiceMissions(ABC):
#     def __init__(self,publicResource,datasets:dict):
#         self.datasets=datasets
#         self.state = MissionsState.WAITING
#         self.foreCheckTimes = -1
#         self.positiveAbortFlag = False
#         self.waitoldtim=0
#         self.waitnewtime=0
#         self.publicResource = publicResource
#         self.StartHigh = self.publicResource.vehicle_odometry.position[2]

#     #给调度器调用的方法
#     @abstractmethod
#     def fore_check(self) -> bool:
#         pass


#     @abstractmethod
#     def done_check(self) -> bool:
#         pass

#     @abstractmethod
#     def abort_check(self):
#         pass

#     @abstractmethod
#     def mission_enter(self):
#         pass
#     @abstractmethod
#     def mission_action(self):
#         pass
#     @abstractmethod
#     def mission_exit(self):
#         pass

#     @abstractmethod
#     def mission_abort(self):
#         pass  

#     #自我调用的方法
#     def mission_blockWait(self, ms:int) -> bool:
#         if self.waitoldtime == 0:
#             self.waitoldtime = self.publicResource.get_clock().now().nanoseconds / 1000000
#         else:
#             self.waitnewtime = self.publicResource.get_clock().now().nanoseconds / 1000000    
#             if self.waitnewtime - self.waitoldtime >= ms:
#                 self.waitoldtime = 0
#                 return True
#             else:
#                 return False
            
#     def positive_abort(self):
#         self.positiveAbortFlag = True
    
#     def positive_abort_check(self):
#         if self.positiveAbortFlag:
#             self.positiveAbortFlag = False
#             return True
#         else:
#             return False

# class ComplexMissions(): #一个组合任务或者复杂任务包括一列飞行任务和一个伴随任务（服务任务)
#     def __init__(self,publicResource, missionsList:list, serviceMissions = None, missionPriority:int = 0) -> None:
#         super().__init__()
#         self.missionsList= missionsList #飞行任务列表
#         self.serviceMissions:ServiceMissions = serviceMissions #伴随任务
#         self.missionsIndex = 0
#         self.servicePoint:int = 0
#         self.publicResourceList = publicResource
#         self.missionPriority = missionPriority #0代表飞行任务优先，非0代表伴随任务优先
#         self.missionFinishedFlag = False
#         self.serviceMissionsFinishedFlag = False
#         self.missionMidFailedFlag = False
#         self.missionFalidFlag = False
#         self.serviceMissionsFalidFlag = False
   
#     def append_missions(self,missions:Missions):
#         missions.publicResourceList = self.publicResourceList
#         self.missionsList.append(missions)

#     def insert_missions(self,missions:Missions):
#         missions.publicResourceList = self.publicResourceList
#         self.missionsList.insert(self.__missionsIndex,missions)
    

# class ComplexMissionsFactory():
#     def __init__(self,publicResource):
#         self.publicResource = publicResource
    
#     def create_missions(self,missionsType:str,datasets:dict) -> Missions:
#         return eval(missionsType)(self.publicResource,datasets)
    
#     def create_serviceMissions(self,serviceMissionsType:str,datasets:dict) -> ServiceMissions:
#         return eval(serviceMissionsType)(self.publicResource,datasets)
#     def create_complexMissions(self,ComplexMissionsJson:str) -> ComplexMissions:
#         ComplexDict=json.loads(ComplexMissionsJson)
#         missionsList = ComplexDict["missionsList"]
#         createMissionsList=[]
#         if missionsList:
#             for missions in missionsList:
#                 createMissionsList.append(self.create_missions(missions["missionsType"],missions["datasets"]))
#         else:
#             return None

#         serviceMissions=ComplexDict["serviceMissions"]
#         if len(serviceMissions) != 0:
#             createServiceMissions = self.create_serviceMissions(missions["serviceMissionsType"],missions["datasets"])
#         else:
#             createServiceMissions = None
        
#         createMissionsPriority = ComplexDict["missionsPriority"]
#         return ComplexMissions(self.publicResource,createMissionsList,createServiceMissions,createMissionsPriority)

